#!/usr/bin/env python
# coding:utf-8
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge, CvBridgeError
import cv2
import sys

# 创建发布者
qr_publisher = rospy.Publisher('QRcode', String, queue_size=10)

def image_callback(msg):
    bridge = CvBridge()
    try:
        # 将ROS的图像消息转换为OpenCV图像
        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
    except CvBridgeError as e:
        rospy.logerr("CvBridge Error: {0}".format(e))

    # 创建二维码检测器
    qr_decoder = cv2.QRCodeDetector()

    # 检测并解码二维码
    data, bbox, _ = qr_decoder.detectAndDecode(cv_image)

    if data:
        rospy.loginfo("QR Code detected: {}".format(data))
        # 发布二维码数据
        qr_publisher.publish(data)
        # 如果检测到二维码，绘制边界框
        if bbox is not None:
            # 将边界框转换为整数
            bbox = bbox.astype(int)
            # 绘制边界框
            cv2.polylines(cv_image, [bbox], True, (0, 255, 0), 2)

    # 展示图像
    cv2.imshow("Camera Image", cv_image)
    cv2.waitKey(3)

def main():
    rospy.init_node('qr_code_reader_node', anonymous=True)
    python_version = sys.version
    rospy.loginfo("Python version: {0}".format(python_version))
    rospy.Subscriber("/camera/color/image_raw", Image, image_callback)
    rospy.spin()

if __name__ == '__main__':
    main()